#include "socket.h"

#include "ros/ros.h"
#include "geometry_msgs/Pose.h"
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>

#include <sstream>
#include <cstring>
using namespace std;

ros::Publisher statusPub;
ros::Publisher posePub;
ros::Publisher gripPub;

Socket *sock = NULL;

void SendIRB(string str)
{
	sock->Send((uint8_t*)str.c_str(), str.length()+1);
}

string RecvIRB()
{
	uint8_t buff[1024];
	size_t len;
	sock->Recv(buff,len,true);

	return (char*)buff;
}

/*!
  This function will always be called once before any other commands are sent, but only once.

  It will initialize the robot to the correct state

  TODO:
 set world
  */
void InitIRB()
{
	static bool runOnce = true;

	// Init stuff (only run once)
	if(runOnce)
	{
		SendIRB("<Mod L wobj0 tool0 z50 v1000>");
		//SendIRB("<Mod L wobj0 tool0 fine v1000>");
		RecvIRB();


		runOnce = false;
	}
}

void strCallback(const std_msgs::String& msg)
{
	/*
	string str = msg.data;

	InitIRB();

	ROS_INFO("Sending: [%s]", str.c_str());

	// Send command to robot
	SendIRB(str);

	string ret = RecvIRB();

	ROS_DEBUG("Recieved: %s", ret.c_str());
	*/

	ROS_WARN("String writes to IRB controller disabled for safety purposes!");
}

void poseCallback(const geometry_msgs::Pose& msg)
{
	InitIRB();

	// Protection against hitting the table
	float z = msg.position.z;

		if(z < 350)
	{
		ROS_WARN("Trying to smash the table!");
				z = 350;
	}

	stringstream ss;

	// Parse move command
	ss << "<Move Target [[" <<
		  msg.position.x << "," <<
		  msg.position.y << "," <<
		  z << "],[" <<
		  msg.orientation.w << "," <<
		  msg.orientation.x << "," <<
		  msg.orientation.y << "," <<
		  msg.orientation.z << "]]>";

	string str = ss.str();

	ROS_INFO("Sending: [%s]", str.c_str());

	// Send command to robot
	SendIRB(str);

	// Wait for command from robot
	string ret = RecvIRB();

	ROS_DEBUG("Recieved: %s", ret.c_str());
}

void getRobotPoseCallback(const std_msgs::Empty&)
{
	InitIRB();

	string str = "<Get Pose>";

	ROS_INFO("Sending: [%s]", str.c_str());

	// Send command to robot
	SendIRB(str);

	// Wait for command from robot
	string ret = RecvIRB();
	ret = ret.substr(ret.find('<')+1, ret.find('>')-ret.find('<')-1);
	ROS_INFO("Recieved: [%s]", ret.c_str());

	stringstream ss(ret);
	geometry_msgs::Pose msg;

	ss.ignore(2);
	ss >> msg.position.x; ss.ignore();
	ss >> msg.position.y; ss.ignore();
	ss >> msg.position.z; ss.ignore(3);
	ss >> msg.orientation.w; ss.ignore();
	ss >> msg.orientation.x; ss.ignore();
	ss >> msg.orientation.y; ss.ignore();
	ss >> msg.orientation.z; ss.ignore();

	posePub.publish(msg);
}

void getRobotGripCallback(const std_msgs::Empty&)
{
	InitIRB();

	string str = "<Get Gripper>";
	ROS_INFO("Sending: [%s]", str.c_str());

	// Send command to robot
	SendIRB(str);

	// Wait for command from robot
	string ret = RecvIRB();
	ret = ret.substr(ret.find('<')+1, ret.find('>')-ret.find('<')-1);
	ROS_INFO("Recieved: [%s]", ret.c_str());

	stringstream ss(ret);

	int val;
	ss >> val;

	std_msgs::Bool msg;
	msg.data = val;

	gripPub.publish(msg);
}

void gripCallback(const std_msgs::Bool& state)
{
	InitIRB();

	string strSet, ret;

	// Check whether to open or close gripper
	if(state.data)
		strSet = "<Set Gripper 1>";
	else
		strSet = "<Set Gripper 0>";

	ROS_INFO("Sending: [%s]", strSet.c_str());

	// Send command to robot
	SendIRB(strSet);

	// Wait for command from robot
	ret = RecvIRB();

	ROS_INFO("Recieved: %s", ret.c_str());

}

int main(int argc, char **argv)
{
	Socket::Init();

	// Socket
	if(argc < 3)
	{
		ROS_ERROR("Fail! Usage: %s <hostname> <port>", argv[0]);
		return 1;
	}

	while(!sock)
	{
		sock = new Socket(argv[1],atoi(argv[2]));
		if(sock->Ok())
			break;
		else
		{
			delete sock; sock = NULL;
			sleep(1);
		}
	}

	ros::init(argc, argv, "controller");

	ros::NodeHandle n;

	// Subscribe to commandBuffers
	ros::Subscriber subPose = n.subscribe("playback", 1000, poseCallback);
	ros::Subscriber subString = n.subscribe("playbackStr", 1000, strCallback);
	ros::Subscriber subGrip = n.subscribe("gripper", 1000, gripCallback);
	ros::Subscriber subGetPose = n.subscribe("getRobotPose", 1000, getRobotPoseCallback);
	ros::Subscriber subGetGrip= n.subscribe("getRobotGrip", 1000, getRobotGripCallback);

	statusPub = n.advertise<std_msgs::String>("status", 1000);
	posePub = n.advertise<geometry_msgs::Pose>("robotPose", 1000);
	gripPub = n.advertise<std_msgs::Bool>("robotGrip", 1000);

	ros::spin();

	return 0;
}
